#!/usr/bin/python3
import rospy
import tf2_ros
from geometry_msgs.msg import Twist
from yolo_pkg.msg import Yolo
import math
import tf
from drone_pkg.msg import SendString
import os

#控制类
class RobotController:

    def __init__(self):

        ####定义常量####
        self.MAX_LINEAR_SPEED = 20              #最大线速度，单位 cm/s
        self.MAX_ANGULAR_SPEED = 15             #最大角速度,单位 度/s

        self.KP_LINEAR_SPEED_POINT = 0.5        #坐标模式线速度KP
        self.KP_LINEAR_SPEED_DETECT = 0.02      #识别模式线速度KP
        self.KP_ANGULAR_SPEED = 0               #角速度环KP

        self.MIN_ERROR_XY_POINT = 4             #坐标模式XY方向允许的最小误差，单位 cm
        self.MIN_ERROR_XY_DETECT = 50           #识别模式XY方向允许的最小误差，单位 像素
        self.MIN_ERROR_YAW = 50                 #YAW上最小角度误差，单位 度、

        ####定义变量####

        #目标变量
        self.targetPointX = 0                   #目标坐标：X，单位 cm
        self.targetPointY = 0                   #目标坐标：Y，单位 cm
        self.targetDetectX = 320                #识别模式X方向目标像素 单位 像素
        self.targetDetectY = 320                #识别模式Y方向目标像素 单位 像素
        self.targetYaw = 0                      #目标角度YAW， 单位度
        self.targetNum = 8                      #目标数字

        #中间变量
        self.distanceXY = 0                     #坐标模式距离目标点距离，单位 cm
        self.errorPointX = 0                    #坐标模式X方向偏差像素值，单位 cm
        self.errorPointY = 0                    #坐标模式Y方向偏差像素值，单位 cm
        self.errorDetectX = 0                   #识别模式X方向偏差像素值，单位 像素
        self.errorDetectY = 0                   #识别模式Y方向偏差像素值，单位 像素
        self.actualPositionX = 0                #实际位置x
        self.actualPositionY = 0                #实际位置y  
        self.actualPositionYaw = 0              #实际位置YAW
        self.detectX = 0                        #识别到数字中心X坐标
        self.detectY = 0                        #识别到数字中心Y坐标
        self.errorYaw = 0                       #Yaw方向误差值，单位 度
        self.cosTheta = 0                       #目标坐标角度余弦值
        self.sinTheta = 0                       #目标坐标角度正弦值
        self.startTime = 0                      #开启时间
        self.presentTime = 0                    #当前时间
        self.dertaTime = 0                      #间隔时间

        #结果变量
        self.resultLinearSpeedXY = 0            #计算输出XY合速度
        self.resultLinearSpeedX = 0             #计算输出X方向速度
        self.resultLinearSpeedY = 0             #计算输出Y方向速度
        self.resultAngularSpeedYaw = 0          #计算输出Yaw方向角速度


        #状态变量
        self.flagMovePoint = 0                  #坐标飞行标志
        self.flagdetect = 0                     #目标点对应的flag
        self.mode = 0                           #飞行模式
        self.detectNum = -1                     #识别到的数字
        self.flagLaser = 0                      #激光灯状态变量，0为关闭， 1为开启

        #初始化ROS节点
        rospy.init_node('control', anonymous=True)
        self.rate = rospy.Rate(10)
        #初始化速度话题发布器
        self.vel_msg = Twist()
        self.pubSpeed = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        #初始化字符串控制话题发布器
        self.sendstr_msg = SendString()
        self.pubString = rospy.Publisher('/send_string', SendString, queue_size=10)

        #初始化TF缓存
        self.tf_buffer = tf2_ros.Buffer()
        tf2_ros.TransformListener(self.tf_buffer)
        rospy.loginfo("Listening for TF data...")
        #初始化订阅YOLO识别
        self.sub_num = rospy.Subscriber("/number_detect", Yolo ,self.dectect_callback, queue_size=10)
        #关灯
        os.system("gpio mode 27 out")
        os.system("gpio write 27 0")


    #计算坐标模式速度
    def calculate_point_speed(self):

        
        #计算距离
        self.distanceXY = math.sqrt(self.errorPointX * self.errorPointX + self.errorPointY * self.errorPointY)

        #计算合速度大小
        self.resultLinearSpeedXY = self.KP_LINEAR_SPEED_POINT * self.distanceXY 
        self.resultLinearSpeedXY = max(min(self.resultLinearSpeedXY, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)

        #计算三角函数
        self.cosTheta = self.errorPointX / self.distanceXY
        self.sinTheta = self.errorPointY / self.distanceXY

        #计算输出速度
        self.resultLinearSpeedX = self.resultLinearSpeedXY * self.cosTheta
        self.resultLinearSpeedY = self.resultLinearSpeedXY * self.sinTheta
        self.resultAngularSpeedYaw = self.KP_ANGULAR_SPEED * self.errorYaw
        
        #速度限幅
        self.resultAngularSpeedYaw = max(min(self.resultAngularSpeedYaw, self.MAX_ANGULAR_SPEED), -self.MAX_ANGULAR_SPEED)
        
    
    #计算识别模式速度
    def calculate_detect_speed(self):
        #计算误差像素
        self.errorDetectX = self.targetDetectX - self.detectX
        self.errorDetectY = self.targetDetectY - self.detectY
        #计算识别后速度
        self.resultLinearSpeedX = self.errorDetectX * self.KP_LINEAR_SPEED_DETECT
        self.resultLinearSpeedY = self.errorDetectY * self.KP_LINEAR_SPEED_DETECT
        #计算限速
        self.resultLinearSpeedX = max(min(self.resultLinearSpeedX, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)
        self.resultLinearSpeedY = max(min(self.resultLinearSpeedY, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)
    
    def dectect_callback(self, number):
        self.detectNum = int(number.Class)
        self.detectX = number.x
        self.detectY = number.y
        pass

    #坐标移动,更改目标坐标点
    #返回为真代表已经到达目的地
    def move(self, x, y, yaw):
        x_act, y_act, yaw_act = self.actualPositionX, self.actualPositionY, self.actualPositionYaw
        self.targetPointX = x
        self.targetPointY = y
        self.targetYaw = yaw
        self.errorPointX = self.targetPointX - x_act   
        self.errorPointY = self.targetPointY - y_act
        self.errorYaw = self.targetYaw - yaw_act
        return (abs(self.errorPointX) <= self.MIN_ERROR_XY_POINT and
                abs(self.errorPointY) <= self.MIN_ERROR_XY_POINT and
                abs(self.errorYaw) <= self.MIN_ERROR_YAW)
    
    #判断是否到达目的地
    def judge_point(self):
        return (abs(self.errorPointX) <= self.MIN_ERROR_XY_POINT and
                abs(self.errorPointY) <= self.MIN_ERROR_XY_POINT and
                abs(self.errorYaw) <= self.MIN_ERROR_YAW)
        
    #判断是否到达识别中心
    def judge_detect(self):
        return (abs(self.errorDetectX) <= self.MIN_ERROR_XY_DETECT and
               abs(self.errorDetectY) <= self.MIN_ERROR_XY_DETECT)
        
        
    #控制激光灯函数， 返回1代表控制完成
    def control_laser(self, time):
        #当激光灯关闭时
        if self.flagLaser == 0:
            #开启激光灯，发送控制话题
            rospy.loginfo("OPENLASER!!!!!!!!!!")
            self.sendstr_msg.Str = '1'
            self.pubString.publish(self.sendstr_msg)
            self.startTime = rospy.get_time()
            self.flagLaser = 1
            os.system("gpio write 27 1")
            return 0
        elif self.flagLaser == 1:
            self.presentTime = rospy.get_time()
            #时间到
            rospy.loginfo("WAITING... Time remaining: %.2f" % (time - (self.presentTime - self.startTime)))
            if (self.presentTime - self.startTime >= time):
                rospy.loginfo("CLOSE!!!!!!!!!!")
                self.sendstr_msg.Str = '0'
                self.pubString.publish(self.sendstr_msg)
                self.flagLaser = 0
                os.system("gpio write 27 0")
                return 1
            else:
                return 0
    


    #控制主程序
    def run(self):
        while not rospy.is_shutdown():
            try:
                transform = self.tf_buffer.lookup_transform('map', 'base_link', rospy.Time(0))

                # 提取位姿数据
                translation = transform.transform.translation
                rotation = transform.transform.rotation

                # 四元数转换为欧拉角
                _, _, yaw = tf.transformations.euler_from_quaternion(
                                (rotation.x, rotation.y, rotation.z, rotation.w))
                self.actualPositionYaw = yaw * 180 / math.pi

                # 获取当前坐标（单位：cm）
                self.actualPositionX = translation.x * 100
                self.actualPositionY = translation.y * 100

                #具体控制写在这#
#############################################___CONTROL_CODE_BEGIN___###############################################################
                
                #当识别到目标数字，切换飞行模式为识别模式
                if self.detectNum == self.targetNum:
                    self.mode = 1
                    
                #走到目标点并识别到数字
                if self.mode == 1 and self.judge_point():
                    self.calculate_detect_speed()
                    rospy.loginfo("DETECT!!!!!!!!!!!!!")
                    #如果定住中心点
                    if self.judge_detect():
                        rospy.loginfo("Open_Laser")
                        # #开启激光灯，并等待5s，之后切换回坐标模式,目标数字改为9
                        if self.control_laser(5):
                            if (self.targetNum == 8):
                                self.mode = 0
                                self.flagMovePoint = 3
                                self.targetNum = 9
                            elif self.targetNum == 9:
                                self.mode = 0
                                pass
                    
                            
                
                #坐标模式
                else:
                    #确保从原点开始
                    if self.flagMovePoint == 0 and self.move(0, 0, 0):
                        self.flagMovePoint = 1
                    #飞往坐标点
                    elif self.flagMovePoint == 1 and self.move(239,-66, 0):
                        self.flagMovePoint = 2
                        
                    elif self.flagMovePoint == 3 and self.move(237, -245,0):
                        self.flagMovePoint = 4
                    
                        
                    
                    #计算速度
                    self.calculate_point_speed()
                        

#############################################___CONTROL_CODE_END___################################################################

                #发布速度,Twist中速度单位为m/s
                self.vel_msg.linear.x = self.resultLinearSpeedX * 0.01
                self.vel_msg.linear.y = self.resultLinearSpeedY * 0.01
                self.vel_msg.angular.z = self.resultAngularSpeedYaw
                self.pubSpeed.publish(self.vel_msg)
                

                #打印调试
                rospy.loginfo("Position:x: %.3f, y: %.3f, yaw: %.3f", self.actualPositionX, self.actualPositionY, self.actualPositionYaw)
                rospy.loginfo("Speed:x: %.3f, y: %.3f, yaw: %.3f", self.resultLinearSpeedX, self.resultLinearSpeedY, self.resultAngularSpeedYaw)
                rospy.loginfo("flag:%d", self.flagMovePoint)
                rospy.loginfo("DetectNum:%d", self.detectNum)
                rospy.loginfo("MOD:%d", self.mode)
                rospy.loginfo("Laser:%d", self.flagLaser)



            except tf2_ros.TransformException as e:
                rospy.logwarn("Transform not available: %s", e)
            self.rate.sleep()

#主函数
if __name__ == '__main__':
    try:
        controller = RobotController()
        controller.run()
    except rospy.ROSInterruptException:
        pass






























